#include "FreeRTOS.h"
#include "semphr.h"
#include "task.h"

#include "global_conf.h"
#include "stm32f4xx.h"
#include "utrasonic.h"
#include "i2c_hal.h"

/*
 * This is ugly but we need for ISR.
 */
static unsigned long capture_value[2];
static unsigned long read_times;
static unsigned long capture;
static struct x_utrasonic data;
static unsigned long isr_count;
static unsigned long capture_ready;

//*******************************************
// API function: rtc_gettime
//description: get current time from RTC
//parametters: rtc: pointer to geting time data buffer
//return : 0 if success
//*******************************************
#define RTC_ADDRESS  0x68
// read/write buffer length
#define Read_Buf_Length  7
#define Write_Buf_Length 8
#define Start_Addr_WR    0

//*****************************************************************************
//----------VARIABLES
//*****************************************************************************
static unsigned char Read_Buf[Read_Buf_Length],Write_Buf[Write_Buf_Length];


unsigned char  rtc_gettime  (struct x_RTC *rtc)
{
    i2c_read(0x68,0x00,Read_Buf_Length, Read_Buf);
    //convert to decimal
    rtc->sec = (Read_Buf[0] & 0x0F) + ((Read_Buf[0] >> 4) & 7) * 10;
    rtc->min = (Read_Buf[1] & 0x0F) + (Read_Buf[1] >> 4) * 10;
    rtc->hour = (Read_Buf[2] & 0x0F) + ((Read_Buf[2] >> 4) & 3) * 10;
    rtc->wday = (Read_Buf[3] & 0x07);
    rtc->mday = (Read_Buf[4] & 0x0F) + ((Read_Buf[4] >> 4) & 3) * 10;
    rtc->month = (Read_Buf[5] & 0x0F) + ((Read_Buf[5] >> 4) & 1) * 10;
    rtc->year = 2000 + (Read_Buf[6] & 0x0F) + (Read_Buf[6] >> 4) * 10;
    return 0;
}
//*******************************************
// API function: rtc_settime
//description: set  time to RTC
//parametters: rtc: pointer to setting time data buffer
//return : 0 if succes
//          1: if not success
//*******************************************
unsigned char  rtc_settime  (struct x_RTC* rtc)
{
    // Write_Buf[0]=0;
	//convert to BCD
    Write_Buf[0] = (unsigned char)((rtc->sec / 10)<<4 | rtc->sec % 10);
	Write_Buf[1] = (unsigned char)((rtc->min / 10 )<<4 | rtc->min % 10);
	Write_Buf[2] = (unsigned char)(rtc->hour / 10) <<4 | rtc->hour % 10;
	Write_Buf[3] = rtc->wday & 7;
	Write_Buf[4] = (unsigned char)(rtc->mday / 10 )<<4 | rtc->mday % 10;
	Write_Buf[5] = (unsigned char)(rtc->month / 10) <<4 | rtc->month % 10;
	Write_Buf[6] = (unsigned char)(((rtc->year - 2000) / 10) <<4 | (rtc->year - 2000) % 10);
    //call initialize sending fuction
    i2c_write(0x68,0x00,Write_Buf_Length, Write_Buf);
      return 0;
}
// end of file


/*
 * RTOS
 */
static xSemaphoreHandle x_read_semaphore;

static unsigned long init_utrasound_pin(struct x_gpio *gpio_struct)
{
    GPIO_InitTypeDef   GPIO_InitStructure;

    /* Enable clock */
    RCC_AHB1PeriphClockCmd(gpio_struct->rcc, ENABLE);
    if (!(gpio_struct->is_input)) {
        /* Configure pin as output */
        GPIO_InitStructure.GPIO_Pin = gpio_struct->pin;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
        GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
        GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    } else {
        /* Configure pin as input floating */
        GPIO_InitStructure.GPIO_Pin = gpio_struct->pin;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
        GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    }
    GPIO_Init(gpio_struct->port, &GPIO_InitStructure);

    if (gpio_struct->is_exti && gpio_struct->is_input) {
        EXTI_InitTypeDef   EXTI_InitStructure;
        NVIC_InitTypeDef   NVIC_InitStructure;

        /* Enable SYSCFG clock */
        RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
        /* Connect EXTI Line to trigger pin */
        SYSCFG_EXTILineConfig(gpio_struct->exti_src_port, gpio_struct->exti_src_pin);

        /* Configure EXTI Line */
        EXTI_InitStructure.EXTI_Line = gpio_struct->exti_line;
        EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
        if (gpio_struct->exti_mode == 1) {
             EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
        } else if (gpio_struct->exti_mode == 2) {
            EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
        } else {
             EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
        }
        EXTI_InitStructure.EXTI_LineCmd = ENABLE;
        EXTI_Init(&EXTI_InitStructure);

        /* Enable and set EXTI Line Interrupt to the lowest priority */
        NVIC_InitStructure.NVIC_IRQChannel = gpio_struct->exti_nvic_channel;
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x01;
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;
        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
        NVIC_Init(&NVIC_InitStructure);
    }
    return 1;
}


static unsigned long init_echo_pin(struct x_gpio *gpio_struct)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    TIM_ICInitTypeDef  TIM_ICInitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    /* TIM1 clock enable */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);

    /* GPIOA clock enable */
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);

    /* TIM1 channel 2 pin (PE.11) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_11;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOE, &GPIO_InitStructure);

    /* Connect TIM pins to AF2 */
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource11, GPIO_AF_TIM1);

    /* Enable the TIM1 global Interrupt */
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY + 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    /*
     *
     */
    TIM_TimeBaseStructure.TIM_Period = 65535;
    TIM_TimeBaseStructure.TIM_Prescaler = UTRASOUND_TIMER_PRESCALE;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
    /* TIM1 configuration: Input Capture mode
     The external signal is connected to TIM1 CH2 pin (PE.11)
     The Rising edge is used as active edge,
     The TIM1 CCR2 is used to compute the frequency value
    */

    TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
    TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
    TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
    TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
    TIM_ICInitStructure.TIM_ICFilter = 0x0;

    TIM_ICInit(TIM1, &TIM_ICInitStructure);

    /* TIM enable counter */
    /* TIM_Cmd(TIM1, ENABLE); */

    /* Enable the CC2 Interrupt Request */
    TIM_ITConfig(TIM1, TIM_IT_CC2, ENABLE);
    return 1;
}

static unsigned long init_echo_timer(void)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    /*
     *
     */
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);

    TIM_TimeBaseStructure.TIM_Period = 65535;
    TIM_TimeBaseStructure.TIM_Prescaler = ECHO_TIMER_PRESCALE;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(ECHO_TIMER, &TIM_TimeBaseStructure);

    /* Enable the TIM7 global Interrupt */
    NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x01;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    /* Enable the Interrupt Request */
    TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
    return 1;
}

static unsigned long start_timer(TIM_TypeDef* timer_id)
{
    TIM_Cmd(timer_id, ENABLE);
    TIM_SetCounter(timer_id, 0);
    EXTI_ClearITPendingBit(ECHO_EXTI_LINE);
    return 1;
}

static unsigned long stop_timer(TIM_TypeDef* timer_id)
{
    TIM_Cmd(timer_id, DISABLE);
    return 1;
}

static unsigned long set_pin_high(struct x_gpio *gpio_struct)
{
    GPIO_SetBits(gpio_struct->port, gpio_struct->pin);
    return 1;
}

static unsigned long set_pin_low(struct x_gpio *gpio_struct)
{
    GPIO_ResetBits(gpio_struct->port, gpio_struct->pin);
    return 1;
}

/*
 * Interrupt handlers
 */

void trigger_isr_handler(void)
{
}

void echo_isr_handler(void)
{
    static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
    if (EXTI_GetITStatus(ECHO_EXTI_LINE) == SET) {
        EXTI_ClearITPendingBit(ECHO_EXTI_LINE);
        if (GPIO_ReadInputDataBit(GPIO_ECHO_PORT,GPIO_ECHO_PIN) == Bit_SET) {
            capture_value[0] = TIM_GetCounter(ECHO_TIMER);
            isr_count = 0;
            read_times++;
        } else {
            read_times--;
            capture_value[1] = TIM_GetCounter(ECHO_TIMER);
            /* Capture computation */
            if (capture_value[1] > capture_value[0]) {
                capture = 0xFFFF * isr_count + (capture_value[1] - capture_value[0]);
            } else if ( (capture_value[1] < capture_value[0])) {
                capture = 0xFFFF * isr_count - (capture_value[0] - capture_value[1]);
            } else {
                capture = 0xFFFF * isr_count;
            }
            capture_ready = 1;
            //xSemaphoreGiveFromISR( x_read_semaphore, &xHigherPriorityTaskWoken );
            //if( xHigherPriorityTaskWoken ) {
                /* taskYIELD_FROM_ISR(); */
            //    taskYIELD();
            //}
        }
    }
}

void echo_timer_isr_handler(void)
{
    if (TIM_GetITStatus(ECHO_TIMER, TIM_IT_Update) == SET) {
        /* Clear interrupt pending bit */
        TIM_ClearITPendingBit(ECHO_TIMER, TIM_IT_Update);
        isr_count++;
    }
}

void utrasound_timer_isr_handler(void)
{
    static signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
    if(TIM_GetITStatus(TIM1, TIM_IT_CC2) == SET) {
        /* Clear TIM1 Capture compare interrupt pending bit */
        TIM_ClearITPendingBit(TIM1, TIM_IT_CC2);
        if(read_times == 0) {
            /* Get the Input Capture value */
            capture_value[0] = TIM_GetCapture2(TIM1);
            read_times++;
        } else if (read_times == 1) {
            /* Get the Input Capture value */
            capture_value[1] = TIM_GetCapture2(TIM1);

            /* Capture computation */
            if (capture_value[1] > capture_value[0]) {
                capture = (capture_value[1] - capture_value[0]);
            } else if (capture_value[1] < capture_value[0]) {
                capture = ((0xFFFF - capture_value[0]) + capture_value[1]);
            } else {
                capture = 0;
            }
            read_times = 0;
            xSemaphoreGiveFromISR( x_read_semaphore, &xHigherPriorityTaskWoken );
            if( xHigherPriorityTaskWoken ) {
                /* taskYIELD_FROM_ISR(); */
                taskYIELD();
            }
        }
    }
}

/*
 * External APIs
 */
unsigned long
open_utrasonic_module (struct x_utrasonic_infos *infos)
{
    if (infos == NULL) {
        return 0;
    }
    infos->data = &data;
    infos->data->is_object = 0;
    infos->data->distance = 0;
    capture_ready = 0;
    isr_count = 0;
    read_times = 0;
    i2c_open();
    /*
     * Initialize trigger pin
     */
    init_utrasound_pin(&(infos->trigger_pin));
    /*
     * Initialize echo pin
     */
    vSemaphoreCreateBinary( x_read_semaphore );
    if( x_read_semaphore == NULL )
    {
        /* Could not create semaphore */
        return 0;
    }
    if ( xSemaphoreTake( x_read_semaphore, ( portTickType ) 10 ) != pdTRUE ) {
        return 0;
    }
    init_utrasound_pin(&(infos->echo_pin));
    init_echo_timer();
    //init_echo_pin(&(infos->echo_pin));
    return 1;
}

unsigned long
start_utrasonic_measure(struct x_utrasonic_infos *infos, \
                    unsigned long (*utrasonic_callback)(struct x_utrasonic *data))
{
    portTickType xDelay = 1 / portTICK_RATE_MS;
    start_timer(ECHO_TIMER);
    set_pin_high(&(infos->trigger_pin));
    vTaskDelay( xDelay );
    set_pin_low(&(infos->trigger_pin));
#if 0
    xDelay = 100/ portTICK_RATE_MS;
    if ( xSemaphoreTake( x_read_semaphore, xDelay ) == pdTRUE ) {
        if ( (capture < 30000) && (capture > 100) ){
            infos->data->is_object = 1;
            infos->data->distance = (unsigned long)(capture * (331.3 + 0.606 * infos->temperature)/2);
        } else {
            infos->data->is_object = 0;
            infos->data->distance = 0;
        }
        /* call the callback function */
        if ( utrasonic_callback !=NULL ) {
            utrasonic_callback(infos->data);
        } else {
            return 0;
        }
    }
    stop_timer(ECHO_TIMER);
#endif
    //xDelay = 50 / portTICK_RATE_MS;
    //if ( xSemaphoreTake( x_read_semaphore, xDelay ) == pdTRUE ) {
        //while(!capture_ready)
        //{
        //}
        capture_ready = 0;
        if ( (capture < 25000) && (capture > 100) ){
            infos->data->is_object = 1;
            infos->data->distance = (unsigned long)((capture * (331.3 + 0.606 * infos->temperature)/2)/10000);
        } else {
            infos->data->is_object = 0;
            infos->data->distance = 0;
        }
        /* call the callback function */
        if ( utrasonic_callback != NULL ) {
            utrasonic_callback(infos->data);
        } else {
            return 0;
        }
    //}
    stop_timer(ECHO_TIMER);
    static unsigned char data;
    static unsigned char r_data[7];
    data = 0;
    //i2c_write(0x1E, 0x02,1, &data);
    //MPU 6050/9150
    //i2c_read(0x68,0x75,1, &data);
    //i2c_read(0x68,0x00,7, r_data);
    struct x_RTC test_rtc, read_rtc;
    test_rtc.sec = 10;
    test_rtc.min = 10;
    test_rtc.hour = 10;
    //rtc_settime(&test_rtc);
    rtc_gettime(&read_rtc);
    return 1;
}

unsigned long
is_object_detected(struct x_utrasonic_infos *infos)
{
    if(infos->data->is_object) {
        return 1;
    } else {
        return 0;
    }
}

unsigned long
get_distance_to_object(struct x_utrasonic_infos *infos)
{
    return(infos->data->distance);
}

unsigned long
close_utrasonic_module (struct x_utrasonic_infos *infos)
{
    if (infos == NULL) {
        return 0;
    }
    return 1;
}
